/**
 * @file:          TrapezoidCurve.h
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2024.12.09
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2024.12.09,12:04:33
 */

#ifndef _TRAPEZOIDCURVE_H_
#define _TRAPEZOIDCURVE_H_

/* Include Files **************************************************************/
#include "Datatype.h"

/* Macro Definition ***********************************************************/

/* Data Type Define ***********************************************************/
enum TrapezoidCurve_PlanMode
{
    TrapezoidCurvePlanMode_Pos = 0,
    TrapezoidCurvePlanMode_Spd,
    TrapezoidCurvePlanMode_PosDoublePlan,
};

enum TrapezoidCurve_TrajMode
{
    TrapezoidCurveTrajMode_ConstantVelocityNoExist = 0,
    TrapezoidCurveTrajMode_ConstantVelocityExist,
};

enum TrapezoidCurve_DoublePlanTrajMode
{
    TrapezoidCurveTrajMode_DoublePlanTraj_Mode1 = 0,
    TrapezoidCurveTrajMode_DoublePlanTraj_Mode2,
    TrapezoidCurveTrajMode_DoublePlanTraj_Mode3,
    TrapezoidCurveTrajMode_DoublePlanTraj_Mode4,
};

typedef struct TrapezoidCurve
{
    /* param */
    uint16_t param_plan_mode;
    float Ts;                    /* Unit: us */
    float param_max_speed_rad_s; /* Unit: rad/s */
    float param_acc_rad_s2;      /* Unit: rad/s^2 */
    uint16_t param_auto_plan;
    uint32_t param_start_delay_count;
    /* for pos double plan mdoe */
    float param_double_plan_max_speed_rad_s[2];
    float param_double_plan_max_acc_rad_s2[2];
    float param_double_plan_1st_constant_speed_distance_rad;

    /* in */
    uint16_t in_start;
    float in_target_pos;
    float in_fdb_pos;
    float in_target_speed;
    float in_fdb_spd;

    uint16_t running_pre;
    uint32_t count;

    /* var */
    uint32_t T_count;
    uint32_t T_count_max;
    float displacement;
    int16_t dir;
    uint16_t traj_type;
    float origin_pos;
    float origin_speed;
    float target_pos;
    float target_pos_last;
    float target_pos_old;
    float target_speed;
    float target_speed_last;
    float target_speed_old;
    float t;
    float t1;
    float t2;
    float t3;
    float tsec1;
    float tsec2;
    float tsec3;
    float T1;
    float T2;
    float T3;
    float s1;
    float s2;
    float s3;
    float v0;
    float v1;
    float v2;
    float v3;
    float a1;
    float a2;
    float a3;

    float Tk1;
    float sk1;
    uint16_t start;
    uint16_t stop;
    uint16_t running;
    uint16_t finished;
    /* for pos double plan */
    int16_t dir_speed;
    float a4;
    float a5;
    float a6;
    float a7;
    float v4;
    float v5;
    float v6;
    float v7;
    float s4;
    float s5;
    float s6;
    float s7;
    float T4;
    float T5;
    float T6;
    float T7;
    float t4;
    float t5;
    float t6;
    float t7;
    float tsec4;
    float tsec5;
    float tsec6;
    float tsec7;
    float Tk2;
    float Tk3;
    float sk2;
    float sk3;

    /* out */
    float out_cmd_pos; /* Unit: rad */
    float out_cmd_spd; /* Unit: rad/s */
    float out_cmd_acc; /* Unit: rad/s^2 */

    /* public method */
    void (*Init)(struct TrapezoidCurve *self, float init_pos);
    float (*Run)(struct TrapezoidCurve *self);
    void (*UpdateParam)(struct TrapezoidCurve *self);

    /* private method */

} TrapezoidCurve;

/* Variable Declare ***********************************************************/

/* Function Declare ***********************************************************/
void TrapezoidCurve_Init(struct TrapezoidCurve *self, float init_pos);
void TrapezoidCurve_UpdateParam(struct TrapezoidCurve *self);
float TrapezoidCurve_Run_PosPlan(struct TrapezoidCurve *self);
float TrapezoidCurve_Run_SpeedPlan(struct TrapezoidCurve *self);
float TrapezoidCurve_Run_PosDoublePlan(struct TrapezoidCurve *self);
#endif
